
%% Saturn
warning off
clc;clear;
format longg
options_negx = odeset('RelTol',1e-13,'AbsTol',1e-13,'Events',@NegXcrossing);
options=odeset('RelTol',1e-13,'AbsTol',1e-13);
options_fmincon = optimoptions('fmincon', 'MaxFunctionEvaluations', 5000, ...
    'MaxIterations', 5000,'ConstraintTolerance',1e-6,'Algorithm','Interior-point','stepTolerance',1e-21);
%% Inputs.
G           = 6.674e-11 * ((1/1000)^3); % Gravitational parameters

mass_central = 1.989e30;
%[] Mass of central planet

mass_moon = 5.972e24;         %Callisto
%[] mass of moons

DU = 151.73e6;           %Callisto
%[km] Semi-major axis of Moons, used as distance units for each CRTBP
%environment.

moonName = {'Luna'};
%[] Name of the Moons

thetao = 0;
%[] Initial position of the moon relative to the first point of aries

GM_central = mass_central*G;
%[] Gravitational parameters

GM_moon = mass_moon*G;
%[] Gravitational parameters

N = length(mass_moon);
%[] Number of Moons

u = zeros(N,1);
for ii = 1:N
    u(ii) = GM_moon(ii)/(GM_moon(ii)+GM_central);
end
%[] Gravataional ratio

TU = zeros(N,1);
VU = zeros(N,1);
for ii = 1:N
    TU(ii) = sqrt(DU(ii)^3/GM_central);
    VU(ii) = DU(ii)/TU(ii);
end
%[] Time constant for each crtbp system

theta_dot = zeros(1,N);
for ii = 1:length(theta_dot)
    theta_dot(ii) = sqrt(GM_central*DU(ii))/DU(ii)^2;
end
%[] Theta dot for each planet.




%% environment settings 
dt = 0.001;
startphase=0; % changing the starting position of the periodic
% orbit to show changes in the pole visibility based on the Satellite's position wrt the Sun's pole
% OpenCRTBP_u(u)
%% Define inertial frame FNs
[xs,ys,zs] = ellipsoid(-0,0,0,696340,696340,696340,500);
figure;
s = surface(xs,ys,zs);
rotate(s, [1 0 0], 7.25);
rotate(s, [0 0 1], 73.67+0.014*(2023-1850));
InclinationRotationAngle = 7.25;
RAANRotationAngle = 73.67+0.014*(2023-1850);
PoleDirection = Rotation([0;0;1],RAANRotationAngle,'Degrees')*...
                Rotation([1;0;0],InclinationRotationAngle,'Degrees')*...
                [0;0;1];
rotate(s, PoleDirection, 0);

FN_i = s.VertexNormals;
%%
load('SE_L4_Vertical.mat');
inc_list = zeros(1,length(T_L4_VL));
for ii = 1:length(T_L4_VL)
    IC = IC_L4_VL(:,ii);
    T = T_L4_VL(ii);
    [t,S] = ode113(@(t,S)CR3BP_n(t,S,u),[0:dt:T],IC,options);
    S = S';

    S_i = zeros(size(S));
    S_i = C2I_primary(S(:,ii),u,DU,VU,t(ii)+5.1+startphase);
    COE_list = State2Coe(S_i,GM_central);
    inc_list(ii) = COE_list(3);
end


%% Simulation paramters %%
CameraAngleCapability=60;
inc_desired = 14.5;
optval = min(abs(inc_list-inc_desired));
[ind_opt] = find(abs(inc_list-inc_desired)==optval);

%%
IC_L4 = IC_L4_VL(:,ind_opt);
T_L4 = 2*pi;
IC_L5 = [IC_L4(1);-IC_L4(2);0;...
         -IC_L4(4);IC_L4(5);IC_L4(6)];

% OpenCRTBP_u(u); hold on
L4= DetermineOptimalL4andL5State(IC_L4,T_L4,u,DU,VU,TU,dt);
L5= DetermineOptimalL4andL5State(IC_L5,T_L4,u,DU,VU,TU,dt);
load('SEL1_Lyapunov.mat');
[t,S] = ode113(@(t,S)CR3BP_n(t,S,u),[0:dt:2*pi],IC(:,1),options);
S=S';
S_i = zeros(6,length(t));
for ii = 1:length(t)
    S_i(:,ii) = C2I_primary(S(:,ii),u,DU,VU,t(ii));
    epoch(ii) = datetime(2023,1,1,0,0,0) + seconds(t(ii)*TU);
end
L1.S_i = S_i;
L1.epoch = epoch;

%%

R_sun = 696340;
h_apex = 100;
theta = asind(R_sun/DU);
alpha = acosd(R_sun/(R_sun+h_apex));
beta = acosd(R_sun/(R_sun+h_apex));
Theta_min = 90-alpha-theta;
Theta_max = 90+beta-theta;

%%

% 1. find min and maximum point for L4 and L5

[t4,S4] = ode113(@(t,S)CR3BP_n(t,S,u),[0:dt:2*pi],IC_L4,options);S4 = S4';
[t5,S5] = ode113(@(t,S)CR3BP_n(t,S,u),[0:dt:2*pi],IC_L5,options);S5 = S5';

[~,ind_maxL4] = find(max(S4(3,:))==S4(3,:));
[~,ind_minL5] = find(min(S5(3,:))==S5(3,:));

S_L4 = S4(:,ind_maxL4);   
S_L5 = S5(:,ind_minL5);   

figure; hold on;
plot3(S4(1,:),S4(2,:),S4(3,:));
plot3(S4(1,ind_maxL4),S4(2,ind_maxL4),S4(3,ind_maxL4),'ro');
plot3(S5(1,:),S5(2,:),S5(3,:));
plot3(S5(1,ind_minL5),S5(2,ind_minL5),S5(3,ind_minL5),'ro');
% 2. given point, determine visible cells on sphere
% 3. plot regions both 3D and 2D


%% Compute L4

S_i = L4.S_i;

temp_time_month = month(L4.epoch);


% plot3(S(1,:),S(2,:),S(3,:))
[row,col,~] = size(FN_i);
visibility_i=zeros(row,col);
for ii = 1:row
    for jj = 1:col
        normvec = [FN_i(ii,jj,1);FN_i(ii,jj,2);FN_i(ii,jj,3)];
        for k = ind_maxL4
            posvec = S_i(1:3,k);
            angle_temp = acosd(dot(normvec,posvec)/(norm(normvec)*norm(posvec)));
            if angle_temp < CameraAngleCapability
                visibility_i(ii,jj) = 1;
            end
            if round(angle_temp)==0
                L4Location=[ii,jj];
            end
        end
    end
end


ImageBinary_L4_on_disk = mat2gray(visibility_i);

S_i = L4.S_i;

temp_time_month = month(L4.epoch);


[row,col,~] = size(FN_i);
visibility_i=zeros(row,col);
for ii = 1:row
    for jj = 1:col
        normvec = [FN_i(ii,jj,1);FN_i(ii,jj,2);FN_i(ii,jj,3)];
        for k = ind_maxL4
            posvec = S_i(1:3,k);
            angle_temp = acosd(dot(normvec,posvec)/(norm(normvec)*norm(posvec)));
            if and((Theta_min<angle_temp),(angle_temp<Theta_max))
                visibility_i(ii,jj) = 1;
            end
        end
    end
end
ImageBinary_L4_limb = mat2gray(visibility_i);


%% Compute L5

S_i = L5.S_i;

temp_time_month = month(L5.epoch);


% plot3(S(1,:),S(2,:),S(3,:))
[row,col,~] = size(FN_i);
visibility_i=zeros(row,col);
for ii = 1:row
    for jj = 1:col
        normvec = [FN_i(ii,jj,1);FN_i(ii,jj,2);FN_i(ii,jj,3)];
        for k = ind_minL5
            posvec = S_i(1:3,k);
            angle_temp = acosd(dot(normvec,posvec)/(norm(normvec)*norm(posvec)));
            if angle_temp < CameraAngleCapability
                visibility_i(ii,jj) = 1;
            end
            if round(angle_temp)==0
                L5Location=[ii,jj];
            end
        end
    end
end


ImageBinary_L5_on_disk = mat2gray(visibility_i);

S_i = L5.S_i;

temp_time_month = month(L5.epoch);


[row,col,~] = size(FN_i);
visibility_i=zeros(row,col);
parfor ii = 1:row
    for jj = 1:col
        normvec = [FN_i(ii,jj,1);FN_i(ii,jj,2);FN_i(ii,jj,3)];
        for k = ind_minL5
            posvec = S_i(1:3,k);
            angle_temp = acosd(dot(normvec,posvec)/(norm(normvec)*norm(posvec)))
            if and((Theta_min<angle_temp),(angle_temp<Theta_max))
                visibility_i(ii,jj) = 1;
            end
        end
    end
end
ImageBinary_L5_limb = mat2gray(visibility_i);


%% Compute L1

S_i = L1.S_i;

temp_time_month = month(L1.epoch);


% plot3(S(1,:),S(2,:),S(3,:))
[row,col,~] = size(FN_i);
visibility_i=zeros(row,col);
for ii = 1:row
    for jj = 1:col
        normvec = [FN_i(ii,jj,1);FN_i(ii,jj,2);FN_i(ii,jj,3)];
        for k = 1
            posvec = S_i(1:3,k);
            angle_temp = acosd(dot(normvec,posvec)/(norm(normvec)*norm(posvec)));
            if angle_temp < CameraAngleCapability
                visibility_i(ii,jj) = 1;
            end
            if round(angle_temp)==0
                earthLocation=[ii,jj];
            end
        end
    end
end


ImageBinary_L1_on_disk = mat2gray(visibility_i);

S_i = L1.S_i;

temp_time_month = month(L1.epoch);


[row,col,~] = size(FN_i);
visibility_i=zeros(row,col);
parfor ii = 1:row
    for jj = 1:col
        normvec = [FN_i(ii,jj,1);FN_i(ii,jj,2);FN_i(ii,jj,3)];
        for k = 1
            posvec = S_i(1:3,k);
            angle_temp = acosd(dot(normvec,posvec)/(norm(normvec)*norm(posvec)));
            if and((Theta_min<angle_temp),(angle_temp<Theta_max))
                visibility_i(ii,jj) = 1;
            end
        end
    end
end
ImageBinary_L1_limb = mat2gray(visibility_i);
%% L4+L5

ImageBinary_L145 =6*ImageBinary_L1_on_disk+5*ImageBinary_L1_limb+...
                  4*ImageBinary_L4_on_disk+3*ImageBinary_L4_limb+...
                  2*ImageBinary_L5_on_disk+1*ImageBinary_L5_limb;
ImageBinary_L145_circle =ImageBinary_L1_on_disk+ImageBinary_L1_limb+...
                  ImageBinary_L4_on_disk+ImageBinary_L4_limb+...
                  ImageBinary_L5_on_disk+ImageBinary_L5_limb;
        
Window = figure('OuterPosition',[100,100,900,500]);
hold on;
mesh(ImageBinary_L145*100,'EdgeColor','interp','FaceColor','interp');
xlabel('\lambda [deg]')
ylabel('\phi [deg]')
titlestring = sprintf('L4 Month = %d',epoch);
% title(titlestring)
set(gca,'fontsize',15)
xticks(linspace(0,row,5));
xticklabels({'180W','90W','0','90E','180E'});
yticks(linspace(0,col,5))
yticklabels({'90S','45S','0','45N','90N'});
xlim([0,row]);
ylim([0,col])
colormap("turbo");
% cd=colorbar();
% cd.Ticks=[40,110,185,260];
% cd.TickLabels={'0','1','2','3'};
% ylabel(cd,"# of observable sats",'FontSize',15,'Rotation',270)
% cd.Label.Position(1)=4;
plot3(earthLocation(2),earthLocation(1),1000,'ro','MarkerSize',15,'MarkerFaceColor','r')
plot3(L4Location(2),L4Location(1),1000,'rs','MarkerSize',15,'MarkerFaceColor','r')
plot3(L5Location(2),L5Location(1),1000,'rd','MarkerSize',15,'MarkerFaceColor','r')

[xs,ys,zs] = ellipsoid(-0,0,0,696340,696340,696340,500);
figure;
hold on;
surface(xs,ys,zs,ImageBinary_L145_circle,'EdgeColor','none','AlphaData',0.5,'FaceAlpha',0.6);
axis equal
colormap("gray")

[xs,ys,zs] = ellipsoid(-0,0,0,696340,696340,696340,500);
figure;
hold on;
surface(xs,ys,zs,ImageBinary_L145,'EdgeColor','none','AlphaData',0.5,'FaceAlpha',0.6);
axis equal



